﻿#include <core\math\mk3d_types.h>

namespace Make3D{
    namespace core {


    public class InterpolatedVector3FArray {
        public enum Interpolation {
            Linear = 1,
            Cosine = 2,
            Cubic = 3
        }
        private delegate Sharp3D.Math.Core.Vector3F interpolator(Sharp3D.Math.Core.Vector3F a, Sharp3D.Math.Core.Vector3F b, float x);

        /// <summary>
        /// Interpolate two values using linear interpolation.
        /// </summary>
        /// <param name="a">A single-precision floating point number representing the first point.</param>
        /// <param name="b">A single-precision floating point number representing the second point.</param>
        /// <param name="x">A single-precision floating point number between 0 and 1 ( [0,1] ).</param>
        /// <returns>The interpolated value.</returns>
        private static Sharp3D.Math.Core.Vector3F LinearInterpolation(Sharp3D.Math.Core.Vector3F a, Sharp3D.Math.Core.Vector3F b, float x) {
            return a * (1 - x) + b * x;
        }
        /// <summary>
        /// Interpolate two values using cosine interpolation.
        /// </summary>
        /// <param name="a">A single-precision floating point number representing the first point.</param>
        /// <param name="b">A single-precision floating point number representing the second point.</param>
        /// <param name="x">A single-precision floating point number between 0 and 1 ( [0,1] ).</param>
        /// <returns></returns>
        private static Sharp3D.Math.Core.Vector3F CosineInterpolation(Sharp3D.Math.Core.Vector3F a, Sharp3D.Math.Core.Vector3F b, float x) {
            float ft = (float)(x * (float)Math.PI);
                   x = (1.0f - (float)System.Math.Cos(ft)) * 0.5f;
                   return a * (1 - x) + b * x;
        }
        /// <summary>
        /// Interpolate two values using cubic interpolation.
        /// </summary>
        /// <param name="a">A single-precision floating point number representing the first point.</param>
        /// <param name="b">A single-precision floating point number representing the second point.</param>
        /// <param name="x">A single-precision floating point number between 0 and 1 ( [0,1] ).</param>
        /// <returns></returns>
        private static Sharp3D.Math.Core.Vector3F CubicInterpolation(Sharp3D.Math.Core.Vector3F a, Sharp3D.Math.Core.Vector3F b, float x) {
            float fac1 = 3 * (float)System.Math.Pow(1 - x, 2) - 2 * (float)System.Math.Pow(1 - x, 3);
            float fac2 = 3 * (float)System.Math.Pow(x, 2) - 2 * (float)System.Math.Pow(x, 3);
            return a * fac1 + b * fac2;
        }


        private interpolator m_interpolator = new interpolator(LinearInterpolation);
        private Sharp3D.Math.Core.Vector3FArrayList vertices = null;
        private int out_points;

        public InterpolatedVector3FArray(int interpolated_points) {
            vertices = new Sharp3D.Math.Core.Vector3FArrayList(10);
            out_points = interpolated_points;
        }

        public InterpolatedVector3FArray(int interpolated_points, Interpolation intp)
            : this(interpolated_points) {

            switch (intp) {
                case Interpolation.Linear:
                    m_interpolator = new interpolator(LinearInterpolation);
                    break;

                case Interpolation.Cosine:
                    m_interpolator = new interpolator(CosineInterpolation);
                    break;

                case Interpolation.Cubic:
                    m_interpolator = new interpolator(CubicInterpolation);
                    break;

            }
        }

        public InterpolatedVector3FArray(int interpolated_points, Interpolation intp, Sharp3D.Math.Core.Vector3FArrayList list )
            : this(interpolated_points, intp) {
            vertices = list;
        }


        public Sharp3D.Math.Core.Vector3F this[int index] {
            get {

                if (vertices.Count == 0) return new Sharp3D.Math.Core.Vector3F();
                else if (vertices.Count == 1) return vertices[0];



                float x = (float)index / (float)out_points;
                x *= vertices.Count;

                int idx = (int)Math.Floor(x);
                x = x - (float)idx;
                int idx1 = idx + 1;

                if (idx1 >= vertices.Count) {
                    return vertices[idx];
                }
                else if (idx1 < 0) {
                    return vertices[0];
                }
                Sharp3D.Math.Core.Vector3F ret = m_interpolator(vertices[idx], (Sharp3D.Math.Core.Vector3F)vertices[idx1], x);

                return ret;
            }
            set {
                //List[index] = value;
            }
        }

        public int Count { get { return out_points; } set { out_points = value; } }

        public void AddControlPoint(Sharp3D.Math.Core.Vector3F value) {
             vertices.Add(value);
        }

        public void RemoveControlPoint(int idx) {
             vertices.RemoveAt(idx);
        }

        public Sharp3D.Math.Core.Vector3F GetControlPoint(int idx) {
            return vertices[idx];
        }

        public int ControlPointsCount { get { return vertices.Count; } }

        public void Clear() {
            vertices.Clear();
        }


        public Sharp3D.Math.Core.Vector3FArrayList getInterpolatedList() {

            Sharp3D.Math.Core.Vector3FArrayList ret = new Sharp3D.Math.Core.Vector3FArrayList(this.out_points);
            for (int i = 0; i < out_points; i++)
                ret.Add(this[i]);

            return ret;
        }


    }
}
